package com.suray.basic.wcs.path.core.api.impl;


import com.suray.basic.wcs.node.WCSNodeModule;
import com.suray.basic.wcs.node.bo.WCSNodeModuleBO;
import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.path.core.Route;
import com.suray.basic.wcs.path.core.api.RouteFactory;
import com.suray.basic.wcs.path.enums.NodeStatus;
import com.suray.basic.wcs.path.enums.NodeType;
import com.suray.basic.wcs.path.exception.CreatePathFailedException;
import com.suray.basic.wcs.path.exception.DifFloorException;
import com.suray.basic.wcs.time.bo.WCSTimeModuleBO;
import com.suray.basic.wcs.utils.Coord;
import com.suray.basic.wcs.utils.LockNode;
import org.springframework.beans.BeanUtils;

import java.util.*;

/**
 * @author Shaozn
 */
public class RouteFactoryImp implements RouteFactory {
    //节点资源分配不合理
    private final static int CHANGE_DISTRICT = 5;
    // 换向消耗
    private final static int TURN_CHANGE = 100;
    /**
     * 目的位置为提升机，只有换层会规划该路径
     */
    private final static long L = 1000000l;

    /**
     * 被锁定的节点
     */
    private final static int LOCK_NODE = 1000;
    /**
     * 当前库区的所有点
     */
    private List<Node> nodes;
    /**
     * 当前库区被锁定的点
     */
    private Set<LockNode> lockedNodes;


    /**
     * 优先队列(升序)
     */
    Queue<Node> openList = new PriorityQueue<>();

    List<Node> closeList = new ArrayList<Node>();

    private int rgvType;

    private WCSTimeModuleBO wcsTimeModuleBO;

    /**
     * 构造器
     *
     * @param nodes       当前库区的所有点
     * @param lockedNodes 当前库区被锁定的点
     * @param rgvType
     */
    public RouteFactoryImp(List<Node> nodes, Set<LockNode> lockedNodes, WCSTimeModuleBO wcsTimeModuleBO, int rgvType) {
        this.nodes = nodes;
        this.lockedNodes = lockedNodes;
        this.wcsTimeModuleBO = wcsTimeModuleBO;
        this.rgvType = rgvType;
    }


    /**
     * 开始算法
     *
     * @return
     * @throws CreatePathFailedException
     */
    @Override
    public synchronized Route createRoute(Node startNode, Node endNode, boolean palletStatus) throws CreatePathFailedException {
        // 重新创建新对象否则会导致下次存在同一节点规划路径会影响
        if (startNode == null) {
            throw new IllegalArgumentException("需要传入起点！");
        }
        if (endNode == null) {
            throw new IllegalArgumentException("需要传入终点！");
        }
        if (startNode.getZ() != endNode.getZ()) {
            throw new DifFloorException(startNode, endNode, "层数不同, 请使用换层接口");
        }
        if (nodes == null) {
            throw new IllegalArgumentException("地图数据不可为空！");
        }
        // 清空上次规划中的节点信息
        openList.clear();
        closeList.clear();
        Node startPoint = getNode(startNode);
        Node endPoint = getNode(endNode);
        if (!canAddNodeToOpen(endPoint, palletStatus, startPoint)) {
            //如果开始位置不能规划
            throw new CreatePathFailedException(startPoint, endPoint, "请检查" + startPoint.toString() + "节点附近是否可行驶");
        }
        // 开始搜索
        openList.add(startPoint);
        do {
            Node current = openList.poll();
            assert current != null;
            addNeighborNodeInOpen(current, endPoint, palletStatus);
            closeList.add(current);
            if (isCoordInClose(endPoint)) {
                return drawPath(startPoint, endPoint);
            }
        } while (!openList.isEmpty());
        throw new CreatePathFailedException(startPoint, endPoint, String.format("请检查!起点：%s，终点：%s，路径是否可行驶", startPoint, endPoint));
    }

    /**
     * 创建提升机节点路径不包括提升机节点
     *
     * @param startNode
     * @param endNode
     * @param palletStatus 穿梭车托盘状态
     * @return
     * @throws CreatePathFailedException
     */
    public synchronized Route createRouteNoLifterNode(Node startNode, Node endNode, boolean palletStatus) throws CreatePathFailedException {
        // 重新创建新对象否则会导致下次存在同一节点规划路径会影响
        if (startNode == null) {
            throw new IllegalArgumentException("需要传入起点！");
        }
        if (endNode == null) {
            throw new IllegalArgumentException("需要传入终点！");
        }
        if (startNode.getZ() != endNode.getZ()) {
            throw new DifFloorException(startNode, endNode, "层数不同, 请使用换层接口");
        }
        if (nodes == null) {
            throw new IllegalArgumentException("地图数据不可为空！");
        }
        // 清空上次规划中的节点信息
        openList.clear();
        closeList.clear();
        Node startPoint = getNode(startNode);
        Node endPoint = getNode(endNode);
        if (!canAddNodeToOpen(endPoint, palletStatus, startPoint)) {
            //如果开始位置不能规划
            throw new CreatePathFailedException(startPoint, endPoint, "请检查" + startPoint.toString() + "节点附近是否可行驶");
        }
        // 开始搜索
        openList.add(startPoint);
        do {
            Node current = openList.poll();
            assert current != null;
            addNeighborNodeInOpenExceptLifterNode(current, endPoint, palletStatus);
            closeList.add(current);
            if (isCoordInClose(endPoint)) {
                return drawPath(startPoint, endPoint);
            }
        } while (!openList.isEmpty());
        throw new CreatePathFailedException(startPoint, endPoint, String.format("请检查!起点：%s，终点：%s，路径是否可行驶", startPoint, endPoint));
    }

    /**
     * 在二维数组中绘制路径
     */
    private Route drawPath(Node startGrid, Node end) {
        if (end == null) {
            return null;
        }
        Route route = new Route();
        long cost = end.G;
        route.setStartGrid(startGrid);
        route.setEndGrid(end);
        List<Node> nodeList = new ArrayList<>();
        while (end != null) {
            nodeList.add(getNode(end));
            end = end.parent;
        }
        route.setCost(nodeList.size() * CHANGE_DISTRICT + cost);
        Collections.reverse(nodeList);
        route.setNodeList(nodeList);
        //重新对消耗值进行计算
        return route;
    }

    /**
     * 添加所有邻结点到open表
     */
    private void addNeighborNodeInOpen(Node current, Node endPoint, boolean palletStatus) {
        Node currentNode = getNode(current);

        if (currentNode.getForbidRgvs().contains(rgvType)) {
            //该节点不允许当前类型的车经过
            return ;
        }

        int x = current.getX();
        int y = current.getY();
        int z = current.getZ();
        if (getNode(current).getUp()) {
            //上（excel中）
            addNeighborNodeInOpen(current, new Node(x - 1, y, z), endPoint, palletStatus);
        }
        if (getNode(current).getDown()) {
            //下（excel中）
            addNeighborNodeInOpen(current, new Node(x + 1, y, z), endPoint, palletStatus);
        }
        if (getNode(current).getLeft()) {
            // 左（excel中）
            addNeighborNodeInOpen(current, new Node(x, y - 1, z), endPoint, palletStatus);
        }
        if (getNode(current).getRight()) {
            // 右（excel中）
            addNeighborNodeInOpen(current, new Node(x, y + 1, z), endPoint, palletStatus);
        }
    }

    /**
     * 添加所有邻结点到open表,除了提升机节点
     */
    private void addNeighborNodeInOpenExceptLifterNode(Node current, Node endPoint, boolean palletStatus) {
        int x = current.getX();
        int y = current.getY();
        int z = current.getZ();
        if (getNode(current).getUp() && getNode(current).getNodeType() != NodeType.L) {
            //上（excel中）
            addNeighborNodeInOpen(current, new Node(x - 1, y, z), endPoint, palletStatus);
        }
        if (getNode(current).getDown() && getNode(current).getNodeType() != NodeType.L) {
            //下（excel中）
            addNeighborNodeInOpen(current, new Node(x + 1, y, z), endPoint, palletStatus);
        }
        if (getNode(current).getLeft() && getNode(current).getNodeType() != NodeType.L) {
            // 左（excel中）
            addNeighborNodeInOpen(current, new Node(x, y - 1, z), endPoint, palletStatus);
        }
        if (getNode(current).getRight() && getNode(current).getNodeType() != NodeType.L) {
            // 右（excel中）
            addNeighborNodeInOpen(current, new Node(x, y + 1, z), endPoint, palletStatus);
        }
    }


    /**
     * 添加一个邻结点到open表
     *
     * @param current      当前点
     * @param grid         相邻点
     * @param endPoint     路径目的点
     * @param palletStatus 托盘状态
     */
    private void addNeighborNodeInOpen(Node current, Node grid, Node endPoint, boolean palletStatus) { // 再此处判断是否可行驶
        long value = 3;
        if (wcsTimeModuleBO != null) {
            current.calculatePassTimeNoTask(wcsTimeModuleBO.getSpeed(), 0, wcsTimeModuleBO.getPalletUpTime(), wcsTimeModuleBO.getPalletDownTime(),
                    wcsTimeModuleBO.getDirectionTime());
            value = current.passTime;
        }
        grid = getNode(grid);
        if (canAddNodeToOpen(grid, palletStatus, endPoint)) {
            //验证是否要换向  相邻点和当前点的父节点进行验证是否在一条直线
            Node parentNode = current.parent;
            if (parentNode != null && !Node.isStraight(grid, parentNode)) {
                value = TURN_CHANGE;
                if (wcsTimeModuleBO != null) {
                    if (Long.MAX_VALUE - wcsTimeModuleBO.getDirectionTime() < value) {
                        value = Long.MAX_VALUE;
                    } else {
                        value += wcsTimeModuleBO.getDirectionTime();
                    }
                }
            }
            if (getNode(current).getNodeType() == NodeType.L) {
                if (Long.MAX_VALUE - L < value) {
                    value = Long.MAX_VALUE;
                } else {
                    value += L;
                }
            }
            if (lockedNodes != null && lockedNodes.size() > 0) {
                for (LockNode lockNode : lockedNodes) {
                    if (current.getX() == lockNode.getX() && current.getY() == lockNode.getY() && current.getZ() == lockNode.getZ()) {
                        if (wcsTimeModuleBO != null) {
                            if (Long.MAX_VALUE - lockNode.getUnLockTime() < value) {
                                value = Long.MAX_VALUE;
                            } else {
                                value = value + lockNode.getUnLockTime() + LOCK_NODE;
                            }
                        } else {
                            value += LOCK_NODE;
                        }
                        break;
                    }
                }
            }
            // 计算邻结点的G值
            long G = current.G;
            if (Long.MAX_VALUE - current.G < value) {
                G = Long.MAX_VALUE;
            } else {
                G = current.G + value;
            }
            if (wcsTimeModuleBO != null) {
                WCSNodeModuleBO wcsNodeModuleBO = WCSNodeModule.wcsNodeModuleMapCache.get(current.toString());
                if (wcsNodeModuleBO != null) {
                    if (wcsNodeModuleBO.getMaxLockTimeWCSNodeModuleRouteBO() != null &&
                            wcsNodeModuleBO.getMaxLockTimeWCSNodeModuleRouteBO().getLockTime() < G && wcsNodeModuleBO.getMaxLockTimeWCSNodeModuleRouteBO().getPassTime() > G) {
                        if (Long.MAX_VALUE - wcsNodeModuleBO.getMaxLockTimeWCSNodeModuleRouteBO().getCurrentPassTime() < G) {
                            G = Long.MAX_VALUE;
                        } else {
                            G = wcsNodeModuleBO.getMaxLockTimeWCSNodeModuleRouteBO().getPassTime();
                        }
                    }
                    if (Long.MAX_VALUE - 2 < G) {
                        G = Long.MAX_VALUE;
                    } else {
                        G = G + 2;
                    }
                }
            }
            Node child = findNodeInOpen(grid);
            if (child == null) {
                // 计算H值
                int H = calcH(endPoint, grid);
                if (isEndNode(endPoint, grid)) {
                    child = endPoint;
                    child.parent = current;
                    child.G = G;
                    child.H = H;
                } else {
                    child = new Node(grid, current, G, H);
                }
            } else if (child.G > G) {
                child.G = G;
                child.parent = current;
            }
            if (!openList.contains(child)) {
                openList.add(child);
            }
        }
    }

    public boolean permitNodeSynRgv(Node node) {
        node = getNode(node);

        if (node.getNodeType() == NodeType.B) {
            return false;
        }

        return true;
    }

    public boolean canStopNodeSynRgv(Node node) {
        node = getNode(node);

        return node.getCanStop();
    }

    /**
     * 从Open列表中查找结点
     */
    private Node findNodeInOpen(Coord coord) {
        if (coord == null || openList.isEmpty()) {
            return null;
        }
        for (Node node : openList) {
            if (node.equals(coord)) {
                return node;
            }
        }
        return null;
    }

    /**
     * 计算H的估值：“曼哈顿”法，坐标分别取差值相加
     */
    private int calcH(Coord end, Coord coord) {
        return Math.abs(end.getX() - coord.getX()) + Math.abs(end.getY() - coord.getY());
    }

    /**
     * 判断结点是否是最终结点
     */
    private boolean isEndNode(Coord end, Coord coord) {
        return coord != null && end.equals(coord);
    }

    /**
     * 判断结点能否放入Open列表
     *
     * @param grid         需要判断的点
     * @param palletStatus 托盘状态
     * @param endPoint     目的点
     * @return
     */
    private boolean canAddNodeToOpen(Node grid, boolean palletStatus, Node endPoint) {
        // 判断是否是不可通过的结点
        if (grid.getNodeType() == NodeType.B) {
            return false;
        }
        // 判断结点是否存在close表
        if (isCoordInClose(grid)) {
            return false;
        }
        // 判断当前是否是载货行驶，并该节点状态为有货状态
        if (!grid.equals(endPoint)) {
            if (palletStatus && grid.getNodeStatus().equals(NodeStatus.Y) && (NodeType.P.equals(grid.getNodeType()) || NodeType.R.equals(grid.getNodeType()))) {
                return false;
            }
        }
        if (grid.getForbidRgvs().contains(rgvType)) {
            return false;
        }
        return true;
    }

    /**
     * 判断坐标是否在close表中
     */
    private boolean isCoordInClose(Coord coord) {
        if (coord == null) {
            return false;
        }
        if (closeList.isEmpty()) {
            return false;
        }
        for (Node node : closeList) {
            if (node.getX() == coord.getX() && node.getY() == coord.getY() && node.getZ() == coord.getZ()) {
                return true;
            }
        }
        return false;
    }

    /**
     * 根据坐标获取对象
     *
     * @param coord
     * @return
     */
    private Node getNode(Coord coord) {
        for (Node node : nodes) {
            if (node.equals(coord)) {
                Node newNode = new Node(coord);
                BeanUtils.copyProperties(node, newNode);
                return newNode;
            }
        }
        throw new RuntimeException("数据无法匹配:" + coord.toString());
    }

    public Node getLatelyNode(Node startNode, List<Coord> availableNodeList, boolean palletStatus) throws CreatePathFailedException {
        // 重新创建新对象否则会导致下次存在同一节点规划路径会影响
        if (startNode == null) {
            throw new IllegalArgumentException("需要传入起点！");
        }
        if (availableNodeList == null) {
            throw new IllegalArgumentException("需要传入终点！");
        }
        if (nodes == null) {
            throw new IllegalArgumentException("地图数据不可为空！");
        }
        openList.clear();
        closeList.clear();
        Node startPoint = new Node(startNode);

        // 开始搜索
        openList.add(startPoint);
        do {
            Node current = openList.poll();
            addNeighborNodeInOpen(current, startPoint, palletStatus);
            for (Coord availableNode : availableNodeList) {
                if (current.getX() == availableNode.getX() && current.getY() == availableNode.getY() && current.getZ() == availableNode.getZ()) {
                    return current;
                }
            }
            if (isCoordInClose(current)) {
                return null;
            } else {
                closeList.add(current);
            }
        } while (!openList.isEmpty());
        return null;
    }
}
